﻿using SocketWeaver.FixedMath;
using System;
using System.Collections.Generic;
using UnityEngine;

namespace SocketWeaver.FPhysics2D
{
    [RequireComponent(typeof(FTransform))]
    [ExecuteInEditMode]
    public class FRigidbody2D : MonoBehaviour
    {
        FTransform _transform;
        public FTransform fTransform
        {
            get
            {
                if (_transform == null)
                {
                    _transform = GetComponent<FTransform>();
                }

                return _transform;
            }
        }
        FCollider2D[] colliders;

        internal FPhysicsBody2D _body2D;
        [SerializeField]
        UInt16 _bodyId;

        [SerializeField]
        internal bool _collisionEvents;

        internal UInt64[] _enterCollisions;
        internal int _enterCollisionCount;

        internal UInt64[] _exitCollisions;
        internal int _exitCollisionCount;

        [SerializeField]
        FCollisionEvent _OnCollisionEnter = new FCollisionEvent();

        [SerializeField]
        FCollisionEvent _OnCollisionStay = new FCollisionEvent();

        [SerializeField]
        FCollisionEvent _OnCollisionExit = new FCollisionEvent();

        [SerializeField]
        internal int sceneIndex;
        public uint externalId;

        //============================== Body properties ==============================
        [SerializeField]
        BodyType _bodyType = BodyType.Dynamic;
        [SerializeField]
        FFloat _linearDrag = FFloat.zero;
        [SerializeField]
        FFloat _angularDrag = FFloat.FromDivision(5, 100);
        [SerializeField]
        FFloat _gravityScale = FFloat.one;
        [SerializeField]
        bool _fixedRotation = false;

        [SerializeField]
        FFloat _mass = FFloat.zero;

        public UInt16 bodyId
        {
            get
            {
                return _bodyId;
            }
        }

        public FFloat mass
        {
            get
            {
                return _body2D.mass;
            }
        }

        public BodyType bodyType
        {
            get
            {
                return _bodyType;
            }
            set
            {
                _bodyType = value;
            }
        }

        public FFloat linearDrag
        {
            get
            {
                return _linearDrag;
            }
            set
            {
                _linearDrag = value;
            }
        }

        public FFloat angularDrag
        {
            get
            {
                return _angularDrag;
            }
            set
            {
                _angularDrag = value;
            }
        }

        public FFloat gravityScale
        {
            get
            {
                return _gravityScale;
            }
            set
            {
                _gravityScale = value;
            }
        }

        public bool fixedRotation
        {
            get
            {
                return _fixedRotation;
            }
            set
            {
                _fixedRotation = value;
            }
        }

        public void UpdateBodyProperties()
        {
            FPhysics2D.UpdateBodyProperties(_body2D, (int)bodyType, linearDrag, angularDrag, fixedRotation, gravityScale);
        }

        //============================== Velocity ==============================
        public FVector2 velocity
        {
            get
            {
                return _body2D.linearVelocity;
            }
            set
            {
                _body2D.linearVelocity = value;
                FPhysics2D.UpdateBodyVelocity(_body2D, value, angularVelocity);
            }
        }

        // rad/sec, z-axis (out of the screen)
        public FFloat angularVelocity
        {
            get
            {
                return _body2D.angularVelocity;
            }
            set
            {
                _body2D.angularVelocity = value;
                FPhysics2D.UpdateBodyVelocity(_body2D, velocity, value);
            }
        }

        //============================== transform ==============================
        public FVector2 position
        {
            get
            {
                return _body2D.position;
            }
            set
            {
                _body2D.position = value;
                FPhysics2D.UpdateBodyTransForm(_body2D, value, rotation);
            }
        }

        public FFloat rotation
        {
            get
            {
                return _body2D.angle * FMath.Rad2Deg;
            }
            set
            {
                _body2D.angle = value * FMath.Deg2Rad;
                FPhysics2D.UpdateBodyTransForm(_body2D, position, _body2D.angle);
            }
        }

        //============================== internal ==============================
        internal void AddPhysicsBody()
        {
            colliders = GetComponentsInChildren<FCollider2D>();
            FVector2 pos = (FVector2)fTransform.position;
            FFloat angle = fTransform.rotation.GetZAngle();

            _body2D = FPhysics2D.AddBody(
                                    (int)bodyType,
                                    pos,
                                    angle,
                                    linearDrag,
                                    angularDrag,
                                    fixedRotation,
                                    gravityScale,
                                    this,
                                    externalId);

            _bodyId = _body2D.bodyID;

            foreach (FCollider2D collider in colliders)
            {
                if (collider.attachedBody != null)
                {
                    continue;
                }

                collider.SetRootGameObject(gameObject);
                FPhysicsShape2D shape = collider.CreateShape(gameObject);

                if (shape == null)
                {
                    Debug.LogError("Failed to create collider shape");
                    continue;
                }

                FFloat mass = FFloat.zero;

                if (collider._overrideMass)
                {
                    mass = collider._customMass;
                }

                FPhysicsFixture2D fixture2D = FPhysics2D.AddFixture(_body2D, shape, collider.density, mass);

                collider.ReceiveFixture(fixture2D, this);
            }

            if (bodyType == BodyType.Static)
            {
                return;
            }

            FPhysics2D.ReadBodyMassInfo(_body2D);

            _mass = _body2D.mass;
        }

        internal FCollider2D FindCollider(byte fixtureId)
        {
            for(int i = 0; i < colliders.Length; i++)
            {
                FCollider2D collider = colliders[i];
                if(collider.fixtureId == fixtureId)
                {
                    return collider;
                }
            }

            return null;
        }

        internal void AddEnterCollision(UInt64 collisionId)
        {
            if(_enterCollisionCount >= FPhysicsConstants.MAX_BODY_COLLISION_COUNT_2D)
            {
                Debug.LogWarning($"Exceeded max body collisions={FPhysicsConstants.MAX_BODY_COLLISION_COUNT_2D}");
                return;
            }

            _enterCollisions[_enterCollisionCount] = collisionId;

            _enterCollisionCount++;
        }

        internal void AddExitCollision(UInt64 collisionId)
        {
            if (_exitCollisionCount >= FPhysicsConstants.MAX_BODY_COLLISION_COUNT_2D)
            {
                Debug.LogWarning($"Exceeded max body collisions={FPhysicsConstants.MAX_BODY_COLLISION_COUNT_2D}");
                return;
            }

            _exitCollisions[_exitCollisionCount] = collisionId;

            _exitCollisionCount++;
        }

        //internal void UpdateCollisionArray()
        //{
        //    int newCollisionCount = _collisionCount;
        //    //remove exit
        //    for(int i = 0; i < _exitCollisionCount; i++)
        //    {
        //        bool found = false;

        //        for(int j = 0; j < _collisionCount; j++)
        //        {
        //            if(_collisions[j] == _exitCollisions[i])
        //            {
        //                _collisions[j] = 0;
        //                found = true;
        //                break;
        //            }
        //        }

        //        if(!found)
        //        {
        //            Debug.LogError($"Failed to find exit collision: {_exitCollisions[i]}");
        //        }
        //    }

        //    newCollisionCount -= _exitCollisionCount;

        //    //add enter
        //    int startCollisionIndex = 0;
        //    for(int i = 0; i < _enterCollisionCount; i++)
        //    {
        //        if(startCollisionIndex >= FPhysicsConstants.MAX_BODY_COLLISION_COUNT_2D)
        //        {
        //            Debug.LogError($"Failed to add enter collision: {_enterCollisions[i]}");
        //        }

        //        for(int j = startCollisionIndex; j < FPhysicsConstants.MAX_BODY_COLLISION_COUNT_2D; j++)
        //        {
        //            if(_collisions[j] == 0)
        //            {
        //                _collisions[j] = _enterCollisions[i];
        //                startCollisionIndex = j + 1;
        //                break;
        //            }
        //        }
        //    }

        //    newCollisionCount += _enterCollisionCount;

        //    //sort
        //    pushZerosToEnd(_collisions, FPhysicsConstants.MAX_BODY_COLLISION_COUNT_2D);
        //    _collisionCount = newCollisionCount;
        //    _enterCollisionCount = 0;
        //    _exitCollisionCount = 0;
        //}

        void pushZerosToEnd(UInt64[] arr, int n)
        {
            // Count of non-zero elements
            int count = 0;

            // Traverse the array. If element encountered is
            // non-zero, then replace the element
            // at index â..countâ.. with this element
            for (int i = 0; i < n; i++)
                if (arr[i] != 0)

                    // here count is incremented
                    arr[count++] = arr[i];

            // Now all non-zero elements have been shifted to
            // front and â..countâ.. is set as index of first 0.
            // Make all elements 0 from count to end.
            while (count < n)
                arr[count++] = 0;
        }

        internal void ResetCollisionEvents()
        {
            _exitCollisionCount = 0;
            _enterCollisionCount = 0;
        }

        internal void InvokeCollisionEvents()
        {
            //exit
            for(int i = 0; i < _exitCollisionCount; i++)
            {
                UInt64 collisonId = _exitCollisions[i];

                FCollision2D fCollision2D = FPhysics2D.GetCollision(collisonId, bodyId);

                _OnCollisionExit.Invoke(fCollision2D);
            }
            

            //enter
            for (int i = 0; i < _enterCollisionCount; i++)
            {
                UInt64 collisonId = _enterCollisions[i];

                FCollision2D fCollision2D = FPhysics2D.GetCollision(collisonId, bodyId);

                _OnCollisionEnter.Invoke(fCollision2D);
            }


            //stay
            IntPtr edge = FPhysics2D.GetContactEdge(_body2D);

            UInt64 contactId = 0;

            while(edge != IntPtr.Zero)
            {
                contactId = 0;
                edge = FPhysics2D.GetContactInfoAndNextContactEdge(edge, ref contactId);

                if(contactId > 0)
                {
                    FCollision2D fCollision2D = FPhysics2D.GetCollision(contactId, bodyId);
                    _OnCollisionStay.Invoke(fCollision2D);
                }
            }
        }

        //============================== Force ==============================
        public void AddForce(FVector2 force, ForceMode2D forceMode2D)
        {
            if(forceMode2D == ForceMode2D.Force)
            {
                ApplyForce(force);
            }
            else
            {
                ApplyLinearImpulse(force);
            }
        }

        public void AddForceAtPosition(FVector2 force, FVector2 worldPoint, ForceMode2D forceMode2D)
        {
            if (forceMode2D == ForceMode2D.Force)
            {
                ApplyForce(force, worldPoint);
            }
            else
            {
                ApplyLinearImpulse(force, worldPoint);
            }
        }

        public void AddTorque(FFloat torque, ForceMode2D forceMode2D)
        {
            if (forceMode2D == ForceMode2D.Force)
            {
                ApplyTorque(torque);
            }
            else
            {
                ApplyAngularImpulse(torque);
            }
        }

        //Apply a force to the center of mass
        void ApplyForce(FVector2 force)
        {
            FPhysics2D.ApplyForceToCenter(_body2D, force);
        }

        //Apply a force at a world point
        void ApplyForce(FVector2 force, FVector2 worldPoint)
        {
            FPhysics2D.ApplyForce(_body2D, worldPoint, force);
        }

        //Apply an impulse to the center of mass. This immediately modifies the velocity.
        void ApplyLinearImpulse(FVector2 impluse)
        {
            FPhysics2D.ApplyLinearImpulseToCenter(_body2D, impluse);
        }

        /// Apply an impulse at a point. This immediately modifies the velocity.
        /// It also modifies the angular velocity if the point of application
        /// is not at the center of mass.
        void ApplyLinearImpulse(FVector2 impluse, FVector2 worldPoint)
        {
            FPhysics2D.ApplyLinearImpulse(_body2D, worldPoint, impluse);
        }

        /// Apply a torque. This affects the angular velocity
        /// without affecting the linear velocity of the center of mass.
        /// z-axis (out of the screen)
        void ApplyTorque(FFloat torque)
        {
            FPhysics2D.ApplyTorque(_body2D, torque);
        }

        /// Apply an angular impulse. This immediately modifies the angular velocity
        void ApplyAngularImpulse(FFloat impulse)
        {
            FPhysics2D.ApplyAngularImpulse(_body2D, impulse);
        }

        //============================== Unity Events ==============================
        void Awake()
        {
            if(_collisionEvents)
            {
                _enterCollisions = new UInt64[FPhysicsConstants.MAX_BODY_COLLISION_COUNT_2D];
                _exitCollisions = new UInt64[FPhysicsConstants.MAX_BODY_COLLISION_COUNT_2D];
            }

            _enterCollisionCount = 0;
            _exitCollisionCount = 0;
        }

        void OnDestroy()
        {
            if (_body2D != null)
            {
                FPhysics2D.DestoryBody(_body2D, this);
            }
        }

        void OnEnable()
        {
            if(_body2D != null)
            {
                FPhysics2D.SetEnabled(_body2D, true);
            }
        }

        void OnDisable()
        {
            if (_body2D != null)
            {
                FPhysics2D.SetEnabled(_body2D, false);
            }
        }

        void Update()
        {
#if UNITY_EDITOR
            if (!Application.isPlaying)
            {
                if (sceneIndex != transform.GetSiblingIndex())
                {
                    sceneIndex = transform.GetSiblingIndex();
                    UnityEditor.EditorUtility.SetDirty(this);
                }
            }
#endif
        }
    }
}
